﻿#include "ifkinematicshandle.h"

#define EPSINON 1e-4  // 精度，小于它就认为是0
#define ALLOW_MAX_DIFF (1 / 180 * pi)  /* 距离上一轴角度的最大允许偏差 */
const double pi = 3.1415926;

IFKinematicsHandle* IFKinematicsHandle::instance = IFKinematicsHandle::getInstance();



IFKinematicsHandle* IFKinematicsHandle::getInstance()
{
    return instance == nullptr ? new IFKinematicsHandle : instance;
}



IFKinematicsHandle::IFKinematicsHandle(): d1(215), a1(109), a2(250.5), a3(115.5), d4(239.5),
    Joint_MIN(QVector<float>(0)),
    Joint_MAX(QVector<float>(0)),
    theta(QVector<float>(0))
{
    /// Matrix4d::Identity()才是生成单位矩阵, Ones()是生成全是1的矩阵
    tcf = Eigen::Matrix4d::Identity();
}



void IFKinematicsHandle::setRobotParameter(double d1, double a1, double a2, double a3, double d4)
{

    this->d1 = d1;
    this->a1 = a1;
    this->a2 = a2;
    this->a3 = a3;
    this->d4 = d4;

}



void IFKinematicsHandle::getRobotParameter(double& d1, double& a1, double& a2, double& a3, double& d4)
{

    d1 = this->d1;
    a1 = this->a1;
    a2 = this->a2;
    a3 = this->a3;
    d4 = this->d4;

}



void IFKinematicsHandle::getRobotTheta(QVector<float>& theta)
{

    theta.clear();
    theta = this->theta;

}



void IFKinematicsHandle::getRobotJointLimit(QVector<float>& Joint_MIN, QVector<float>& Joint_MAX)
{

    Joint_MIN = this->Joint_MIN;
    Joint_MAX = this->Joint_MAX;

}



std::vector<float> IFKinematicsHandle::getOffsetFromZeroPoint(const JointType& joint)
{

    std::vector<float> offset = {0, 0, 0};

    switch(joint) {
        case BASE:
            offset = {0, 0, 0};
            break;
        case J1:
            offset = {0, 0, d1};
            break;
        case J2:
            offset = {a1,0, d1};
            break;
        case J3:
            offset = {a1, 0, d1+a2};
            break;
        case J4:
            offset = {a1+d4, 0, d1+a2+a3};
            break;
        case J5:
            offset = {a1+d4, 0, d1+a2+a3};
            break;
        case J6:
            offset = {a1+d4, 0, d1+a2+a3};
            break;
    }

    return offset;

}


/// parameters : d1 a1 a2 a3 d4 θ1 θ2 θ3 θ4 θ5 θ6 j1 j1 j2 j2 j3 j3 j4 j4 j5 j5 j6 j6 tcfx tcfy tcfz
///      index : 0  1  2  3  4  5  6  7  8  9  10 11 12 13 14 15 16 17 18 19 20 21 22 23   24   25
void IFKinematicsHandle::recvParameters(QVector<float> parameters)
{

    if(parameters.size() != 26) return;

    /// 接受dh参数
    this->d1 = parameters[0];
    this->a1 = parameters[1];
    this->a2 = parameters[2];
    this->a3 = parameters[3];
    this->d4 = parameters[4];

    /// 清空数组 接受θ角
    this->theta.clear();
    for (int i = 0; i < 6; ++i) {

        theta.push_back(parameters[5+i]);

    }

    /// 清空数组 接受各关节极限值
    this->Joint_MIN.clear();
    this->Joint_MAX.clear();
    for (int i = 0; i < 12; i+=2) {

        this->Joint_MIN.push_back(parameters[11+i]);
        this->Joint_MAX.push_back(parameters[11+i+1]);

    }

    this->tcf(0, 3) = parameters[23];
    this->tcf(1, 3) = parameters[24];
    this->tcf(2, 3) = parameters[25];

}




Eigen::Vector3d IFKinematicsHandle::getPositionInWorld(const Joint & joint)
{

    Eigen::Matrix4d world = fKine(joint);

#if 0
    joint.print();
    qDebug() << QStringLiteral("末端坐标: ") << world(0, 3) << world(1, 3) << world(2, 3);
#endif

    return {world(0, 3), world(1, 3), world(2, 3)};

}


/* 角度值转弧度值 */
static inline double toRadian(double degree)
{
    return (degree / 180.0)* pi ;
}

static inline double toDegree(double radian)
{
    return (radian / pi) * 180.0;
}


/** 获得当前关节值计算出来的工具坐标点在世界坐标下下的坐标
 * @joint 各关节角度
 * @return 4*4位姿矩阵 在世界坐标系下
*/
Eigen::Matrix4d IFKinematicsHandle::fKine(const Joint & joint) const
{

    Eigen::Matrix4d robot = fKineRobot(joint);

    /// 因为osg默认世界坐标系原点在0,0,0, 而我们机器人算法的世界坐标系原点在1轴正上方,
    /// 所以有一个d1的偏移量, 需要转换成我们算法坐标系中的坐标位置
    Eigen::Matrix4d world_2_robot;
    world_2_robot << 1, 0, 0, 0,
                    0, 1, 0, 0,
                    0, 0, 1, d1,
                    0, 0, 0, 1;

    /// 旋转矩阵的mapping功能
    /// 将机器人坐标系中的点映射为全局世界坐标系中,用矩阵左乘实现,
    Eigen::Matrix4d world = world_2_robot * (robot * tcf);

    return world;
}



/* 正解函数
* @joint 6轴各关节值结构体，单位角度
* @return 返回一个4*4的位姿矩阵
*/
Eigen::Matrix4d IFKinematicsHandle::fKineRobot(const Joint & joint) const
{

    double j1 = toRadian(joint.j1),
           j2 = toRadian(joint.j2),
           j3 = toRadian(joint.j3),
           j4 = toRadian(joint.j4),
           j5 = toRadian(joint.j5),
           j6 = toRadian(joint.j6);

    double s1 = sin(j1), c1 = cos(j1),
           s2 = sin(j2), c2 = cos(j2),
           s3 = sin(j3), c3 = cos(j3),
           s4 = sin(j4), c4 = cos(j4),
           s5 = sin(j5), c5 = cos(j5),
           s6 = sin(j6), c6 = cos(j6);

    Eigen::Matrix4d T01, T12, T23, T34, T45, T56, T06;

    T01 << c1,    0, -s1, a1 * c1,
           s1,    0,  c1, a1 * s1,
            0,   -1,   0,       0,
            0,    0,   0,       1;
    T12 << c2,  -s2,   0, a2 * c2,
           s2,   c2,   0, a2 * s2,
            0,    0,   1,       0,
            0,    0,   0,       1;
    T23 << c3,    0, -s3, a3 * c3,
           s3,    0,  c3, a3 * s3,
            0,   -1,   0,       0,
            0,    0,   0,       1;
    T34 << c4,    0,  s4,       0,
           s4,    0, -c4,       0,
            0,    1,   0,      d4,
            0,    0,   0,       1;
    T45 << c5,    0, -s5,       0,
           s5,    0,  c5,       0,
            0,   -1,   0,       0,
            0,    0,   0,       1;
    T56 << c6,  -s6,   0,       0,
           s6,   c6,   0,       0,
            0,    0,   1,       0,
            0,    0,   0,       1;

    T06 = T01 * T12 * T23 * T34 * T45 * T56;

    return T06;

}


Joint IFKinematicsHandle::iKine(const Eigen::Matrix4d & pose)
{

    Joint joint;
    /// 因为osg默认世界坐标系原点在0,0,0, 而我们机器人算法的世界坐标系原点在1轴正上方,
    /// 所以有一个d1的偏移量, 需要转换成我们算法坐标系中的坐标位置
    Eigen::Matrix4d robot_2_word;
    robot_2_word << 1, 0, 0, 0,
                    0, 1, 0, 0,
                    0, 0, 1, -d1,
                    0, 0, 0, 1;

    /// 将世界坐标系下的位姿矩阵映射到机器人坐标系下,映射用左乘他们之间的转换矩阵
    Eigen::Matrix4d robot = robot_2_word * (pose * tcf.inverse());

    joint = iKineRobot(robot, lastJoint);
    return joint;

}


static void normalize(Eigen::Vector3d& vec)
{

    double n = sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
    vec[0] /= n;
    vec[1] /= n;
    vec[2] /= n;

}


/* 逆解函数
* @Joint 6轴各关节值结构体，单位弧度
* @lastJoints 上一次计算的关节值结构体
* @pose 要解逆解的位姿矩阵
*/
Joint IFKinematicsHandle::iKineRobot(const Eigen::Matrix4d & pose, Joint lastJoints)
{

    /// 将姿态矩阵的旋转矩阵单位化可以防止无解出现
    Eigen::Vector3d nVec = {pose(0, 0), pose(1, 0), pose(2, 0)};
    Eigen::Vector3d oVec = {pose(0, 1), pose(1, 1), pose(2, 1)};
    Eigen::Vector3d aVec = {pose(0, 2), pose(1, 2), pose(2, 2)};
    /// eigen库自带的归一化函数链接不上,只能自造轮子了
    normalize(nVec);
    normalize(oVec);
    normalize(aVec);

    double nx = nVec[0], ox = oVec[0], ax = aVec[0],
           ny = nVec[1], oy = oVec[1], ay = aVec[1],
           nz = nVec[2], oz = oVec[2], az = aVec[2],
           px = pose(0 ,3), py = pose(1, 3), pz = pose(2, 3);

//    double nx = pose(0, 0), ox = pose(0, 1), ax = pose(0, 2),
//           ny = pose(1, 0), oy = pose(1, 1), ay = pose(1, 2),
//           nz = pose(2, 0), oz = pose(2, 1), az = pose(2, 2),
//           px = pose(0 ,3), py = pose(1, 3), pz = pose(2, 3);

    /** joint 1 */
    double j1[2] = {0, 0};
    j1[0] = -atan2(-py, px);
    j1[1] = -atan2(-py, px) + pi;


    /** joint 3 */
    double j3[4] = {0, 0, 0, 0};
    /** temp, x, y, z, A are temp valiables */
    double temp[2], A[2];
    double x = a2 * d4,
           y = a2 * a3,
           z = a2 * a2 + a3 * a3 + d4 * d4;
    for (int i = 0; i < 4; i+=2) {
        temp[i/2] = a1 - px * cos(j1[i/2]) - py * sin(j1[i/2]);
        A[i/2] = z - pz * pz - temp[i/2] * temp[i/2];
        j3[i]   = -atan2(-y, x) + atan2(A[i/2],sqrt( (4 * x * x + 4 * y * y - A[i/2] * A[i/2])));
        j3[i+1] = -atan2(-y, x) + atan2(A[i/2],-sqrt((4 * x * x + 4 * y * y - A[i/2] * A[i/2])));
    }


    /** joint 2 */
    double j2[4] = {0, 0, 0, 0};
    double m[4]  = {0, 0, 0, 0};
    double n[4]  = {0, 0, 0, 0};
    for (int i = 0; i < 4; ++i) {
        m[i] = a2 + a3 * cos(j3[i]) - d4 * sin(j3[i]);
        n[i] = a3 * sin(j3[i]) + d4 * cos(j3[i]);
        j2[i] = atan2(-pz * m[i] + n[i] * temp[i/2],-pz * n[i] - m[i] * temp[i/2]);
    }


    /** joint 5 */
    double j5[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    for (int i = 0; i < 4; ++i) {
        m[i] = (ax * cos(j1[i / 2]) * cos(j2[i]) + ay * sin(j1[i / 2]) * cos(j2[i]) + az * -1 * sin(j2[i]));
        n[i] = -(ax * cos(j1[i / 2]) * sin(j2[i]) + ay * sin(j1[i / 2]) * sin(j2[i]) - az * -1 * cos(j2[i]));
    }
    for (int i = 0; i < 8; ) {
        x =  n[i/2] * cos(j3[i/2]) - m[i/2] * sin(j3[i/2]);
        y = sqrt(pow((ay * cos(j1[i / 4]) - ax * sin(j1[i / 4])),2) + pow((m[i/2] * cos(j3[i/2]) + n[i/2] * sin(j3[i/2])),2));
        j5[i++] = atan2( y, x);
        j5[i++] = atan2(-y, x);
    }


    /** joint 4 */
    double j4[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    for (int i = 0; i < 8; ++i) {
        if (fabs(sin(j5[i])) >= EPSINON) {
            y = ((ay*cos(j1[i/4])-ax*sin(j1[i/4]))) / sin(j5[i]);
            x= (-m[i/2]*cos(j3[i/2])-n[i/2]*sin(j3[i/2]))/(sin(j5[i]));
            j4[i] = atan2(y , x);
        } else {
            j4[i] = 0;
        }
    }


    /** joint 6 */
    double j6[8] = {0, 0, 0, 0, 0, 0, 0 ,0};
    for (int i = 0; i < 8; ++i) {
        double u = nx * sin(j1[i/4]) - ny * cos(j1[i/4]);
        double v = ox * sin(j1[i/4]) - oy * cos(j1[i/4]);
        j6[i] = atan2(cos(j4[i]) * u - cos(j5[i]) * sin(j4[i]) * v, cos(j4[i]) * v + cos(j5[i]) * sin(j4[i]) * u);
    }

    /** 8组逆解结果 */
    double ans[8][6];
    for (int i = 0; i < 8; ++i) {
        ans[i][0] = j1[i / 4];
        ans[i][1] = j2[i / 2];
        ans[i][2] = j3[i / 2];
        ans[i][3] = j4[i];
        ans[i][4] = j5[i];
        ans[i][5] = j6[i];
    }

    /** 在选择8组解中的最优解，主要根据三个原则：连续原则、限位原则、最短路径原则 ，连续原则暂时不使用 */
    double last[6] = {toRadian(lastJoints.j1), toRadian(lastJoints.j2), toRadian(lastJoints.j3),
                      toRadian(lastJoints.j4), toRadian(lastJoints.j5), toRadian(lastJoints.j6)};
    bool isOutLimit[8] = {false, false, false, false, false, false, false, false};
    //bool isContinuity[8] = {true, true, true, true, true, true, true, true};

    /** 排除掉超过运动范围的解 */
    for (int i = 0; i < 8; ++i) {
        for (int j = 0; j < 6; ++j) {
            if (ans[i][j] < toRadian(Joint_MIN[j]) || ans[i][j] > toRadian(Joint_MAX[j])) {
                isOutLimit[i] = true;
                break;
            }
        }
    }

    /** 运动连续：排除掉离上一轴角度过大的解，需要设定最大允许偏差值 ALLOW_MAX_DIFF */
//    for (int i = 0; i < 8; ++i) {
//        for (int j = 0; j < 6; ++j) {
//            if (abs(ans[i][j] - last[j]) > ALLOW_MAX_DIFF) {
//                isContinuity[i] = false;
//                break;
//            }
//        }
//    }

    /** 最短路径原则：选出距离上一次姿态各轴所需最小的运动量，给各轴分配一个权重，使尽量动小轴或姿态轴 */
    double weight[6] = {0.2, 0.25, 0.2, 0.15, 0.12, 0.08};
    //double weight[6] = {1, 1, 1, 1, 1, 1};  // 各轴相同权重
    double sum[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    double minSum = INT_MAX;
    int opt = -1;
    for (int i = 0; i < 8; ++i) {
        if (isOutLimit[i]) {
            qDebug() << QString(QStringLiteral("第%d解超限\n")).arg(i+1);
            continue;
        }
        for (int j = 0; j < 6; ++j) {
            sum[i] += fabs(ans[i][j] - last[j]) * weight[j];
        }
        if (sum[i] < minSum) {
            minSum = sum[i];
            opt = i;
        }
    }

    /** 最优解 */
    Joint res;
    if (opt >= 0) {
        /// 因为4轴和6轴平行,因此4轴转-x°和6轴转x°效果相同,因此如果遇到这种情况就将他们矫正
        if (-ans[opt][3] - ans[opt][5] <= 1e-2) {
            ans[opt][3] = 0;
            ans[opt][5] = 0;
        }
        res = Joint(toDegree(ans[opt][0]), toDegree(ans[opt][1]), toDegree(ans[opt][2]),
                    toDegree(ans[opt][3]), toDegree(ans[opt][4]), toDegree(ans[opt][5]));
    } else {
        qDebug() << QStringLiteral("无合适逆解\n");
        res =  lastJoints;
    }

    return res;

}
